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I. INTRODUCTION 

The fundamental goal of submarine control is to reach and 
maintain ordered depth. Experimental designs involve expensive 
model testing such as Darpa Suboff Model (DTRC Model 5470) 
(Ref. 6]. Much research has been done in depth control of 
submarines [Ref. 3,5]. Our goal is to develop an analytic 
method to determine the stability properties of a design. 

The stability of a design will have a significant impact 
on its responsiveness. A vehicle with a large margin to 
instability will not be very responsive. The problem becomes 
one of determining how close to the margins we can get without 
a total loss of stability. By expanding the scope of our 
research to include nonlinear terms we are able to define the 
limits of stability and therefore margins. 

Previous studies analyzed stability properties of the 
system, specially static bifurcations [Ref. 2) and 
bifurcations to periodic solutions [Ref. 1]. The latter study 
which is used as a basis for this work, was restricted to 
level, zero trim flight paths. 

The purpose of this thesis is to develop a program for 
finding the limits of stability for an out of trim submarine 
at moderate and high speeds. These limits are mapped using a 


Hopf bifurcation analysis program included in the Appendix. 


pee EQUATIONS OF MOTION 
The motion of the submersible in the vertical plane can be 
modeled by four coupled nonlinear differential equations for 
pitch rate (q), heave velocity (w), pitch angle (9) and heave 
(z). With a body fixed coordinate frame at the vehicle's 
geometric center , we can express Newton's equations of motion 


as 


m (w - Ug - 269° - xg4) = Zit Zypo+Z, 5, +Z,8,+ Zw Zo 


(2) 
I (w - xq)” 
-—p {f C,b(<) —————_ ax 
2°f : |w - xq | 
I,-mxg(w-Uq) -Zgwqm = “X gpBcos 8 -zepBsinO+M, 8, 
° ° l W-X jz (2.2) 
My,8,* Mai + My + Mg + Maw + —p [Cb (x) Ende 
2 Jw — xq | 
(2.3) 
0=q 
z = -Usin®+wcos 0 (ae 


Equations 2.1 through 2.2 can be written in a more compact 


form as, 


, : 2 
Wo = a,,Uwta,,Uq + a,,Zgpsin® + a,,xgpcos 0+ b,U 6 


5 255) ) 
b,U 5+ d (wig) + ¢,(w.q) 
G = a,,Uwta,.Uq+ a,,zGnsin 8 + a,,xqpcos 0 + b,U*8 
(26) 
b,U75,+ d,(W,q) + ¢,(W,q) 
where, 
D, = (m ~£Z,) 1, -M ,) - (mx gt Z ,) (mx g+M,,) 
a,,D, = (2, -2C pF _Utan 05) (1,-M,)* (mx gt Z,)(M,,* 2C jE ,U tan By) 
aD, = = (m+Z, + 2C pF ,Utan OJ) (1, aa) 
(ime g*Z,)(M- m X G-MZG U tan ©, 7 p= »U tan 8 ,) 
aD, = (M,,+ 2Cp)E,Utan 8) (m - Z,) + (mx g+ Mi(Z,,- 2C pE_U tan 8,) 
aD, = (M q7 MX g- mz ,U tan 0, -2Cp£,U tan 85) 
* (max g+ My)(m + Z,+ 20 Be FENG 0) 
eZ. / ) 
453D, = -(m+Z,,)B 


a,3D, = ~ (mx ot Z,)B 


PD, = ,-M)Z,— (gt Z,)M, 


b,D, = (m-Z,)My+ (mx gt+M,) Z, 


d,(w.q)D, (m - Zy)1,* (mx ot M,)I, 


d.(w.q)D, 


(1,-M )1,- (mx g* Z,)1, 


c,(W,q/D = (1,-M)mzgq°- (mx + Z ,)mzwq 


cyo(w.qg)D, = -(m-Z,)mzowa - (mx ot M,)mzoq 2 


In these equations the submersible is assumed to be 
neutrally buoyant (W=B), and statically stable (z, > 2z,). Here 
we can assume z, to be zero, hence Zg, = Ze. 

At steady state the cross flow drag integral terms I, and 


I, have the form, 


Ig = -Cpw |w| fb) ax Ip = Cpw|w| [ b(x) xdx (2.8) 


From equation (2.3) it is seen that w is equal to tanO, at 
steady state. The /b(x)dx term is computed numerically for the 
SUBOFF model as E,, and /b(x)xdx term as E, Therefore, the 
cross flow drag terms become, 


I, = -C, w jw/ E, I, = C,w jw/ E, (2.9) 


Because we have two rudders at the bow and the stern, our 
system of equations is multi-input. To reduce this system into 
a single input system the linear combination of the control 
inputs will be modified into the following forn, 

O= 06 (2.10) 


Ss 7 


O, = QO, 
where a is the control surface coordination gain. The value 
of a~ ranges from -1 to 1. The selection of the value of a will 
allow the planes to operate as desired for the particular 
maneuvering condition, i.e., a = 0 for no bow plane control, 
a = -] for bow plane and stern plane opposed to each other, 
yielding the maximum pitch moment, and a = 1 for bow and stern 


plane control in the same direction, yielding the maximum 


heave force. 





FIGURE 2.1 Geometric representations of the basic 


Gen iietomar 


iit. CONTROL LAW 


A. INTRODUCTION 


The control design problem can be expressed in state space 


as follows, 


x= Ax + BS Cae 


where the state vector is 


0 
w 
x = | 
q (a2 ) 
Zz 
Equation 3.1 in our case is, 
6 0 0 I 0 9 
" 4132 on- 007k, a,u-b uk, a, yi -b uk, -b u*k, Ww (3.3) 
a 2 2 2 2 
d Qo Gp-Oytk, ay -by*k, any -byrk, -by ky |9 


=u 1 0 0 yl’ 


Our aim is to find a controller which will assure us a 


stable closed loop system. The only control input is the dive 


plane angle, 6. 


B. FEEDBACK CONTROL 

1. Pole Placement 

The full state feedback controller is a linear function of 
the states and has the form, 

O=- Kx (3.4) 
where K is the vector of feedback gains which are to be 
determined in order to give the desired closed loop system 
dynamics. Substituting equation 2.12 into 2.10 yields, 

g = (A- BK )x (a 
The feedback gains K must be chosen such that A - BK has the 
desired eigenvalues. The actual characteristic equation of the 
closed loop system is given by, 

det (A-- 8K = ss a— 0 (3.6) 
The required values of K are obtained by matching coefficients 


in the two polynomials of the actual and the desired 


characteristic equations. Equation 3.5 becomes, 


6 0 0 1 Of fg 0 
2 
4;%Gp 44 Aypt OF lwl [oyu 
id = te 2 6 ( 3 r 7 
q Qr%Gp Az aA," 0} Iq bu 
|Z | L —-Uu I 0 0 Z 0 | 


The characteristic equation of the closed loop system is, 


-§ 0 1 0 
GZ gp- bj 7k, a,u-bu*k,-s a, - bu *k; -b uk, rR 
det : 
2 2 Zz y 
Azz Ggp-by*k, ayn -by*k, ayy -byk;-s -by’k, 
| —U 1 0 as 


which reduces to, 


s4+(A,k,+A,k,-E,)s3+ (-B,k,- Bk,- B3k,- Byk,- E,)s*+ 


(-C)k,- C,k,- C,k,- E,)s+ (-D,k,- Dyk,) = 0 (3.9) 
where, 
A, = -B, = b, wu’ 
A, = -B, = b,u’ 
fe—"(a,,"b,"- a,, bs) u? 
eee, — (a), b, = a,, b,) u? 
Meee — (a,, b; -ay; b, ) Zn Ue (22110 ) 
m= (a,, b, + b, - a,, b,) Ww 


mo (2a), Dy ~ 22; Dy) u“ 


D 

me (oy, + a,,)eu 

Eee, 26, + (2;,8,, — @,, 2&5) u* 

Fig = (8,3 Az; ~ @,; 423) 2g,_ U 

Now, let's assume that we want to place the closed loop poles 


at -p,, -P,, -P3;, ~p, to have the desired system response. Then 


the desired characteristic equation is, 


4 3 /; (3-11) 


s +@,S° + &58 +@%,5+a,= 0 


where, 


YO = Py * P2 * P3 + YP, 


& 
| 


= P;P2 +t P; P3 +P: Ps + P2P3 + P2 Py + DIP, (35 Ze 
3 = PP, P2P3; + P; P2P, + P; P3 Ps + P2 P3 Py 


O% = P; P2 P3 Py, 


The feedback gains can now be computed by equating the 
coefficients of equation 3.9 and 3.11, 

An Kk; +A, ko. = =c, eee 

ByaKy +°B; kjet+oBy ky +e Bp kee (3e139 
Cie Gat Ken eee. =). ee | 
(D, + D,) ky = @ 

We established the method for placing the poles of the 
system, but we also need to know the desired locations of the 
poles. 

2. Pole Location Selection 

In a typical second order system control law design , 
transient response specifications are given. This results in 
an allowable region in the s-plane where the desired location 
of the poles can be obtained. For higher order systems the 
concept of dominant roots can be employed. In selecting poles 
the physics of the system must be considered. If the poles are 
too negative, a small time constant will result, and the 


system may not be able to react that fast. If we use big gains 


10 


K, this also means that the control effort will be large. In 
practice there are limits based on actuator size and 
saturation. 

Considering the control law design to stabilize the 
submarine to a level flight path at 90 = 0 it is required that 
the submarine return to the level flight, after some small 
disturbances in 98 or z, within the time it takes for the 
vehicle to travel three ship lengths. Since our model is 14 
feet long and its velocity is 5 feet/sec., the required 
recovery time is about 10 seconds. This means the time 
constant is 3 seconds and the closed loop poles should be 
placed at approximately -0.3. 

After placing the poles using equation 3.13 the 
control law is found to be, 


5 = 0.9917 @ + 0.8333 w + 0.6026 g - 0.0351 z (3.14) 


C. FEEDFORWARD CONTROL 

The previous discussion on feedback controller assures 
closed loop stability, but it acts as a regulator in other 
words takes all the states to zero values. If we have constant 
disturbances or we want to track some reference value other 


than zero we can not do this with state feedback alone. 


1] 
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Feeaterward gain 
Linearized modei of piant 


vector of feedback gains 


FIGURE 3.1 Feedback and feedforward control application to 


our linearized model 


In the case of non-zero set points or constant 
disturbances we again need to have the exact same full state 
feedback to ensure closed loop stability. But we also need to 
introduce an additional term to our controller in the form 


U = Kew oe (Jee 


vout 


outt 


where k, is the constant feedforward term. k, is given as 
[Ref. 4], 

k, = tia(0 x, (3.16) 
where x, is the reference values of the states and H,"(s) is 
the closed loop transfer function, 

H.’(s) = C (sI - A + BK)” B (S217) 
Another way of getting k, is looking at the steady state 
equations of motion. In steady state all the time derivatives 


in equations 2.1 through 2.4 go to zero and we have, 


ae (3.18) 
(oro) 

Z,5+Z w-I, = 0 
=(X,- Xp) Bcos 0,-2,,Bsin8,+ M5 fe Meo Ji ah (3.20) 


If the equation 3.19 is multiplied with M, and equation 3.20 
with Z, and set equal to each other and plug in the equation 


3.18, we have an equation depending only on @. 


(Z,M,- M,Z.) tan 0,+ x ~BZ,cos 0,+ zoBZ,sin B+ 


C p(E gM, - E,2,) tan 9, |tan 0, | (or) 


Where E, and E, are the integral terms computed numerically for 
the Suboff model. The steady state value of 90, is 
found from equation 3.20 by using a Newton-Raphson method 


in Bifurl program in the Appendix. Then we can get 6 from 
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equation 3.19, 


_ Le av _ -C p)£E, tan 0, |tan 8,| - Z,,w 


25 Zs 


(3.22) 


After getting 5, we easily find k, from equation 3.15, 
k, = 0 + Kx ( 3202 
A plot of the steady state angle 9, versus x, for different 


values of z, is shown in Figure 3.2. 
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IV. BIFURCATION ANALYSIS 


A. STABILITY 
The nonlinear equations of motion in the dive plane 2.1 
through 2.4 can be expressed in a compact form as follows, 
%= f(x) oo 
Where x is the state variable vector x=[08,w,q,z]. We know 


that the equilibrium points, x, of the system are defined by, 


£(X,) = O | (4.2) 

This is a nonlinear system of algebraic equations and it 

may have multiple solutions in x,, which means that the 
nonlinear system may have more than one positions of static 
equilibrium. If we pick one equilibrium, x, we can establish 
its stability properties by linearization. The linearized 


system becomes, 
4.3 
xX =Ax ( 


where A is the Jacobian matrix of f£ (x) evaluated at x, 


== (4.4) 
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and the state has been defined to designate small deviations 
from the equilibrium x,, 

X —+ X—Xy (4.5) 

In system dynamics as long as all eigenvalues of A have 

negative real parts, we know that the linear system will be 

stable. This means that the equilibrium x, will be stable for 

the nonlinear system as well. This is in fact Lyapunov's 


linearization technique. 


B. BIFURCATION 

Values of the nonlinear system parameters at which the 
qualitative nature of the system's motion changes are known as 
critical or bifurcation values. The phenomena of bifurcation, 
1.e., quantitative change of parameters leading to qualitative 
change of system properties, is the topic of bifurcation 
theory. Euler buckling (Pitchfork bifurcation), limit cycles 
(Hopf bifurcation) are common examples of bifurcation. 

Classical definition of stability states, that the real 
part of all the eigenvalues of the system must be negative. 
Therefore, our initial investigations into the stability of 
the SUBOFF model was to find those eigenvalues whose real 
parts cross the imaginary axis. We used the bifurcation 
analysis program, included in the Appendix, to calculate the 


eigenvalues of the system. 
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By linearizing the equations of motion, equations 2.1 
through 2.4 , the state space equations of the dynamic system 
can be written in the form, 


X=Ax+Bu (4.5) 


where, 

u=- K x (4.6) 
and K is the vector of controller gains, as calculated by pole 
placement in equations 3.13. The eigenvalues of the system are 
found by solving, | 
det |A-BK-sI| = 0 (4.7) 
In the bifurcation program a pseudo-root locus method is 
employed where the time constant, T,, is fixed. The constant 
T. fixes to placement of the system poles at a given nominal 
forward speed U, and then the model speed, U, is varied 
incrementally with the system eigenvalues calculated at each 
speed increment. When the real part of an eigenvalue changes 
sign between the limits of a speed increment a bisection 
method is employed to find the speed where the real part of 

the eigenvalue equals zero. 
For each point where the real part of an eigenvalue 
crosses the imaginary axis the associated T, and U are plotted 


on a bifurcation map. This map delineates the regions of 
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Classical stability (all eigenvalues on the left hand plane) 
from the regions of instability. A family of bifurcation maps 
were generated by varying nominal speed, U,, initial 
stability, z,,, and longitudinal center of gravity/buoyancy 
separation, x, of the submersible. 

Figure (4.1) shows a typical bifurcation map with its five 
distinct regions [Ref. 1]. Region I is the area of classical 
stability. In region II there is one real positive eigenvalue 
which is indicative of a pitchfork bifurcation. Pitchfork 
bifurcations of this model were previously examined by Reidel 
(Ref. 1]. Regions III,IV, and V have at least one pair of 
complex conjugate eigenvalues with a positive real part. This 
would indicate that there should be an unstable oscillatory 


behavior for the model. 


C. RESULTS AND DISCUSSION 

The classical stability region in bifurcation maps lies 
between pitchfork and Hopf bifurcation boundaries. The limits 
or parameters must be defined for the system designer prior to 
starting the design. By maximizing the region of stability we 


can give the designer the most leeway in his work. There are 
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TIME CONSTANT 


PITCHFORK B. 





FORWARD VELOCITY (NONDIMENSIONAL) 


FIGURE 4.1 A typical bifurcation map showing the five 


distinct regions 


two parameters that we used to change the bifurcation maps, 
the longitudinal separation between center of gravity and 
center of buoyancy, x,, and the initial stability, 2Z,,. 

First we look at the change in x,. In figures 4.2 through 
4.7 we plotted bifurcation curves for different initial 
stabilities as x,, varies. We can see that as x,, 1ncreases 
the Hopf bifurcation branches Hl an H2 move towards higher 
speeds and time constants and thus increasing the stability 
area. The H3 branch however remains constant. 

The other important point that we observed is that the 
system becomes unstable at nominal speed at higher time 
constants. This is unexpected because we are designing around 
our nominal speed. A more careful examination in the trimmed 
case shows that the actual forward velocity becomes, Vu*+w’. 
Therefore the system may become stable at a value of u other 
than nominal. 

The next parameter we examined was the initial stability, 
Zope Figures 4.8 through 4.14 show the effect of varying 2Z,, 
from .2 to .4 £t for different x,, values. The H3 branch 
remains constant while the upper speed H2 branch moves down 
effectively decreasing the area of stability. The low speed 


curve Hl moves upwards and increases the low speed area of 


stability. 


Zh 
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FIGURE 4.2 Bifurcation map as xg changes between xg=0 and 


Me 2 ee — ao 
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VELOCITY (NONDIMENSIONAL) 


FIGURE 4.3 Bifurcation map as xg changes between xg=0 and 


me-.2, zg=.2. 
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FIGURE 4.4 Bifurcation map as xg changes between xg=0 and 


xg=-.3, zg=.3. 
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TIME CONSTANT 





VELOCITY (NONDIMENSIONAL) 


FIGURE 4.5 Bifurcation map as xg changes between xg=0 and 


xg=.3, zg=.3. 
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FIGURE 4.6 Bifurcation map as xg changes between xg= 


xg=~.3, zg=.4. 
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FIGURE 4.7 Bifurcation map as xg changes between xg=0 and 


Xxg=.3, zg=.4. 
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FIGURE 4.8 The effects of changing zg on the bifurcatic 


maps, xg=.3. 
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FIGURE 4.9 The effects of changing zg on the bifurcation 


maps, xg=.2. 
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FIGURE 4.10 


maps, xg=.l. 
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The effects of changing zg on the bifurcatio 
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FIGURE 4.11 The effects of changing zg on the bifurcation 


maps, xg=0. 
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FIGURE 4.12 


xg=-.1. 
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The effects of changing zg on the bifurcatio 
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FIGURE 4.13 The effects of changing zg on @e *Sifurecation 


maps, xg=-.2. 
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FIGURE 4.14 The effects of changing zg on the bifurcatue 


maps, xg=-.3. 


V. CONCLUSIONS AND RECOMMENDATIONS 


A. CONCLUSIONS 

Hopf bifurcation analysis is a very useful design tool in 
the design and evaluation phase. Hopf bifurcation analysis and 
an identification program that can evaluate the hydrodynamic 
coefficients for the submersible vehicle will be very useful 
and save money and time by reducing the amount of model 
testing. An effective set of control system parameters can be 
generated in this process that will be optimal for the final 
design of the submersible. 

This type of analysis can set the limits of the ranges of 
important parameters such as metacentric height’ and 
longitudinal separation of buoyancy/gravity centers. As we 
have seen changes in these two parameters can have dramatic 
effects on stability. It was found that the moderate speed 
region of stability increases with increasing metacentric 
height. The same is not true, however, for high speeds. The 
longitudinal separation of center of gravity/buoyancy can have 
a profound effect on stability. It was found that the vehicle 
may be unstable even at nominal speed. This was attributed to 
the fact that at high trim angles, the feedback gains which 


are computed at zero trim, can no longer guarantee stability. 
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B. RECOMMENDATIONS 
The bifurcation analysis program should be expanded to 
evaluate the performance of the submarine including effects of 


external forces such as wave effects, currents, and free 


surface effects. 
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APPENDIX - BIFURCATION ANALYSIS PROGRAM 


C PROGRAM BIFUR1.FOR 
C BIFURCATION ANALYSIS 
C PARAMETERS ARE: TC VS. U 


IMPLICIT DOUBLE PRECISION (A-H,O-Z) 
DOUBLE PRECISION K1,K2,K3,K4,L.MQDOT,MWDOT,MQ,MW,MDS,MDB,MD, 


& MASS, IY,P1,P2,P3,P4,XGB,ZGB 
DIMENSION A(4,4),FV1(4),IV1(4),ZZZ(4,4), WR(4),W1(4),XL(25), 
& BR(25), VEC0(25), VEC1(25), VEC2(25) 


COMMON P1,P2,P3,P4 


OPEN (11,FILE="BIF1.RES'STATUS="NEW’) 

OPEN (12,FILE='BIF2.RES',STATUS="NEW’) 

OPEN (13,FILE='"BIF3.RES'STATUS="NEW’) 
C NUMERIC INFO OF DARPA SUBOFF MODEL 

WEIGHT=1556.2363 

BUO =1556.2363 


ee 13.9792 

i =561.32 

mr 632.2 

MASS =WEIGHT/G 
RHO =1.94 

XB =0.0 

ZB =0.0 

CD =0.4 
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CD =0.5*CD*RHO 


WRITE (*,*) 'ENTER MIN, MAX, AND INCREMENTS IN Tc (nondim)' 
READ (*,*) TCMIN,TCMAX,ITC 
WRITE (*,*) 'ENTER MIN, MAX, AND INCREMENTS IN U (nondim)' 
READ (*,*) UMIN,UMAX,IU 
C WRITE (*,*) 'ENTER NOMINAL SPEED' 
C READ (*,*) U0 
WRITE (*,*) 'ENTER XG AND ZG' 
READ (*,*) XG,ZG 
U0=9 


ZGB=ZG-ZB 
XGB=XG-XB 
TCMIN=TCMIN#*L/U0 
TCMAX=TCMAX*L/U0 
UMIN =UMIN*U0 
UMAX =UMAX*U0 

C HYDRODYNAMIC COEFFICIENTS 
ZQDOT=-6.3300E-04*0.5*RHO*L**4 
ZWDOT=-1.4529E-02*0.5*RHO*L**3 
ZQ =7,.5450E-03*0.5*RHO*L**3 
ZW =-1.3910E-02*0.5*RHO*L**2 
ZDS =-5.6030E-03*0.5*RHO*L**2 
ZDB =-5.6030E-03*0.5*RHO*L**2 
MQDOT=-8.8000E-04*0.5*RHO*L**5 
MWDOT=-5.6100E-04*0.5*RHO*L**4 
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MQ =-3.7020E-03*0.5*RHO*L**4 
MW =1.0324E-02*0.5*RHO*L**3 
MDS =-2.4090E-03*0.5*RHO*L**3 
MDB = 2.4090E-03*0.5*RHO*L**3 


XL(1)=0.0 
XL(2)=0.1 
XL(3)=0.2 
XL(4)=0.3 
XL(5)=0.4 
XL(6)=0.5 
XL(7)=0.6 
XL(8)=0.7 
XL(9)=1.0 
XL(10)=2.0 
XL(11)=3.0 
XL(12)=4.0 
pa3)=7:7143 
XL(14)=10.0 
XL(15)=15.1429 
XL(16)=16.0 
XL(17)=17.0 
XL(18)=18.0 
XL(19)=19.0 
XL(20)=20.0 
XL(21)=20.1 
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XL(22)=20.2 
XL(23)=20.3 
XL(24)=20.4 
XL(25)=20.4167 
DO 102 N=1,25 
XL(N) = (L/20.)*XL(N) - L/2. 
102 CONTINUE 
BR(1)=0.0 
BR(2)=0.485 
BR(3)=0.658 
BR(4)=0.778 
BR(5)=0.871 
BR(6)=0.945 
BR(7)=1.010 
BR(8)=1.060 
BR(9)=1.18 
BR(10)=1.41 
BR(11)=1.57 
BR(12)=1.66 
BR(13)=1.67 
BR(14)=1.67 
BR(15)=1.67 
BR(16)=1.63 
BR(17)=1.37 
BR(18)=0.919 
BR(19)=0.448 
BR(20)=0.195 


BR(21)=0.188 
BR(22)=0.168 
BR(23)=0.132 
BR(24)=0.053 
BR(25)=0.0 
DO 104 K=1,25 
VEC0(K)=BR(K) 
VEC 1(K)=XL(K)*BR(K) 
VEC2(K)=XL(K)*XL(K)*BR(K) 
104 CONTINUE 
CALL TRAP(25,VECO,XL,EO) 
CALL TRAP(25,VEC1,XL,E1) 
CALL TRAP(25, VEC2,XL,E2) 


ALPHA=0.0 
ZD=ZDS+ALPHA*ZDB © 
MD=MDS+ALPHA*MDB 


C CALCULATING THE SS PITCH ANGLE 6, 
C WITH NEWTON RAPHSON METHOD 

P1= ZW*MD - MW*ZD 

P2= XG*BUO*ZD 

P3= ZG*BUO*ZD 

P4= CD*(MD*E0 - ZD*E]1) 

WRITE(*,*) P1,P2,P3,P4 

EPSI=.00000001 

THETA0=0 
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IF(XGB.GT.0) P4=-1*P4 
18 DO 19 I=1,2000 
FT=FUNC(THETAO) 
DFT=DFUNC(THETAO) 
DELT=FT/DFT 
THETA0=THETA0-DELT 
IF(ABS(DELT)-EPSI) 20,20, 19 
19 CONTINUE 
FT=FUNC(THETAO) 
20 WRITE(*,*) THETAO , FT 
C 
DV=(MASS-ZWDOT)*(IY-MQDOT)-(MASS*XG+ZQDOT)*(MASS*XG+MWDOT) 
A11DV=(IY-MQDOT)*(ZW-2*CD*EO*U*TAN(THETAO)) 
& +(MASS*XG+ZQDOT)*(MW+2*CD*E1*U*TAN(THETAO)) 


A12DV=(IY-MQDOT)*(MASS+ZQ+2*CD*E1*U*TAN(THETAO))+ 
&  (MASS*XG+ZQDOT) 
& *(MQ-MASS*XG-MASS*ZG*U*TAN(THETAO)-2*CD*E2*U*TAN(THETAO)) 
A13DV=-(MASS*XG+ZQDOT)*WEIGHT 
BIDV =(IY-MQDOT)*ZD+(MASS*XG+ZQDOT)*MD 
A21DV=(MASS-ZWDOT)*(MW+2*CD*E1*U*TAN(THETAO)) 
& +(MASS*XG+MWDOT)*(ZW-2*CD*EO*U*TAN(THETAO)) 


A22DV=(MASS-ZWDOT)*(MQ-MASS*XG-MASS*ZG*U*TAN(THETAO)-2*CD*E2*U 
& *TAN(THETAO))+(MASS*XG+MWDOT)* 
& (MASS+ZQ+2*CD*E1*U*TAN(THETAO)) 
A23DV=-(MASS-ZWDOT)*WEIGHT 
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B2DV =(MASS-ZWDOT)*MD+(MASS*XG+MWDOT)*ZD 


Al1=A11DV/DV 
A12=A12DV/DV 
A13=A13DV/DV 
A21=A21DV/DV 
A22=A22DV/DV 
A23=A23DV/DV 
Bl =BIDV /DV 

B2 =B2DV /DV 


EPS =1.D-5 
ILMAX=1500 


DO 1 I=1,ITC 
WRITE (*,2001) LITC 
TC=TCMIN+(I-1)*(TCMAX-TCMIN)/(ITC-1) 
POLE=1.0/TC 
ALPHA3=4.0*POLE 
ALPHA2=6.0*POLE**2 
ALPHA 1=4.0*POLE**3 
ALPHAO= POLE**4 
K4=ALPHA0/((B1*A21-B2*A1 1)*U0**4+(B1*A23-B2*A13)*ZGB*U0**2) 
A2M=B1*U0**2 
A3M=B2*U0**2 
AOM=-(A11+A22)*U0-ALPHA3 
BIM=B2*U0**2 
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B2M=(B2*A 12-B1*A22)*U0**3 
B3M=(B1*A21-B2*A11)*U0**3 
BOM=(A11*A22-A21*A12)*U0**2-A23*ZGB-ALPHA2-B1*U0*U0*K4 
CIM=(B2*A11-B1*A21)*U0**3 

C2M=(B1*A23-B2*A13)*ZGB*U0**2 
COM=(A13*A21-A23*A11)*ZGB*U0+ALPHA1-(B2+B 1*A22-B2*A 12)*K4*U0**3 
K2=C1M*BOM*A3M-B1M*COM*A3M-C1M*B3M*A0M 
K2=K2/(C1M*B2M*A3M-B1M*C2M*A3M-C1M*B3M*A2M) 
K1=(COM-C2M*K2)/C1M 

K3=(AOM-A2M*K2)/A3M 


DO 2 J=1,IU 
U=UMIN+(J-1)*(UMAX-UMIN)/(IU-1) 

A(1,1)=0.0D0 

A(1,2)=0.0D0 

A(1,3)=1.0D0 

A(1,4)=0.0D0 
A(2,1)=-A13*(XGB*SIN(THETA0)-ZGB*COS(THETAO))+B1*U*U*K1 
A(2,2)=A11*U_ +B1*U*U*K2 

A(2,3)=A12*U_ +B1*U*U*K3 

A(2,4)= — -B1*U*U*K4 
A(3,1)=-A23*(XGB*SIN(THETA0)-ZGB*COS(THETA0))+B2*U*U*K1 
A(3,2)= A21*U +B2*U*U*K2 

A(3,3)= A22*U +B2*U*U*K3 

A(3,4)= B2*U*U*K4 

A(4,1)=- U 

A(4,2)= 1.0D0 
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A(4,3)= 0.0D0 
A(4,4)= 0.0D0 


CALL RG(4,4,A,WR,WL0,ZZZ,IV1,FV1,IERR) 
CALL DSTABL(DEOS,WR,WIFREQ) 


IF (J.GT.1) GO TO 10 
DEOSOO= DEOS 
UOO = U 
LL= 0 
GO TO 2 

10 DEOSNN = DEOS 
UNN = U 
PR= DEOSNN*DEOSOO 
IF (PR.GT.0.D0) GO TO 3 
LL = LL+1 
IF (LL.GT.3) STOP 1000 
IL = 0 
UO = UOO 
UN = UNN 
DEOSO = DEOSOO 
DEOSN = DEOSNN 

6 UL =UO 
UR = UN 
DEOSL = DEOSO 
DEOSR = DEOSN 

C U = (UL+UR)/2.D0 
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ALPHA = (DEOSL-DEOSR)/(UL-UR) 
U =- (DEOSL-ALPHA*UL)/ALPHA 


A(1,1) = 0.0D0 
A(1,2) = 0.0D0 
A(1,3) = 1.0D0 
A(1,4) = 0.0D0 


A(2,1) = A13*(XGB*SIN(THETAO)-ZGB*COS(THETAO))+B 1*U*U*K1 
A(2,2) = All*U +B1*U*U*K2 

A(2,3 )= Al2*U +B1*U*U*K3 

A(2,4 )= B1*U*U*K4 

A(3,1) = A23*(XGB*SIN(THETAO)-ZGB*COS(THETAO))+B2*U*U*K1 
A(3,2) = A21*U +B2*U*U*K2 

A(3,3) = A22*U +B2*U*U*K3 

A(3,4)= B2*U*U*K4 

A4 i= vi 

A(4,2) = 1.0D0 

A(4,3) = 0.0D0 

A(4,4 )= 0.0D0 


CALL RG(4,4,A, WR, WI,0,ZZZ,1V1,FV1,IERR) 
CALL DSTABL(DEOS, WR, WI,FREQ) 


DEOSM = DEOS 

UM =U 

PRL = DEOSL*DEOSM 
PRR = DEOSR*DEOSM 
IF (PRL.GT.0.D0) GO TO S$ 
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UO = UL 
UN = UM 
DEOSO = DEOSL 
DEOSN = DEOSM 
IL = IL+1 
IF (IL.GT.ILMAX) STOP 3100 
DIF = DABS(UL-UM) 
IF (DIF.GT.EPS) GO TO 6 
U = UM 
GO TO 4 
5 IF (PRR.GT.0.D0) STOP 3200 
UO = UM 
UN = UR 
DEOSO = DEOSM 
DEOSN = DEOSR 
IL = IL+1 
IF (IL.GT.ILMAX) STOP 3100 
DIF = DABS(UM-UR) 
IF (DIF.GT.EPS) GO TO 6 
U = UM 
4 LLL = 10+LL 
WRITE (LLL,*) U/U0,TC*UO0/L 
3. UOO = UNN 
DEOSOO = DEOSNN 
2 CONTINUE 
1 CONTINUE 
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2001 FORMAT (215) 
END 


SUBROUTINE DSTABL(DEOS, WR,WI,OMEGA) 
IMPLICIT DOUBLE PRECISION (A-H,O-Z) 
DIMENSION WR(4),WI(4) 
DEOS=-1.0D+20 
DO 11=1,4 
IF (WR(I).LT.DEOS) GO TO 1 
DEOS = WR(1) 
=I 
| CONTINUE 
OMEGA = WI(IJ) 
OMEGA = DABS(OMEGA) 
RETURN 
END 


SUBROUTINE TRAP(N,A,B,OUT) 
C NUMERICAL INTEGRATION ROUTINE USING THE TRAPEZOIDAL RULE 
IMPLICIT DOUBLE PRECISION (A-H,O-Z) 
DIMENSION A(1),B(1) 
Nl = N-l 
OUT = 0.0 
DO 1I=1,NI 
OUT = 0.5*(A(I+A(I+1))*(B(I+1)-B(D) 
OUT = OUT+OUTI 
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| CONTINUE 
RETURN 
END 


C FUNCTIONS USED IN NEWTON RAPHSON ROUTINE 
FUNCTION FUNC(THETAO) 
IMPLICIT DOUBLE PRECISION (A-H,0-Z) 
COMMON P1,P2,P3,P4 
FUNC = P1*DTAN(THETAO) + P2*DCOS(THETAO) + P3*DSIN(THETAO) 
& +P4*(DTAN(THETAO))**2 
RETURN 
END 


FUNCTION DFUNC(THETAO) 
IMPLICIT DOUBLE PRECISION (A-H,O-Z) 
COMMON P1,P2,P3,P4 
DFUNC= P1*(1.D0/DCOS(THETAO))**2 - P2*DSIN(THETAO) + 
& P3*DCOS(THETAO) + P4*2.D0*DTAN(THETAO0)*(1.D0/DCOS(THETAO))**2.D0 
RETURN 
END 
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